#ifndef __CODE8_IMU_H
#define __CODE8_IMU_H

#include "Code0_CONF.h"
#include "Code0_PARAMETER.h"
						 
#define ACCELER_ADDR 		0x30
#define MAGNETIC_ADDR		0x3C
#define GYRO_ADDR				0xD0

#define ACC_BASE				30
#define GYRO_BASE				36
#define MAG_BASE				33

#define PI								3.1415926
#define RADIAN_TO_DEGREE	57.2957795147199       // radian to degree

#define mtype float
																													
typedef enum
{
	UPRIGHT 				= 0,
	FALLEN_FORWARD 	= 1,
	FALLEN_BACKWARD = 2,
	FALLEN_RIGHT		= 3,
	FALLEN_LEFT			= 4,	
} PostureStatus;

typedef struct
{
	unsigned char Buff[AXIS_NUM * 2];
	float Raw[AXIS_NUM];
	float	q_[AXIS_NUM+1][1];
  float	q[AXIS_NUM+1][1];
	float Max[AXIS_NUM];
	float Min[AXIS_NUM];
	float P_[AXIS_NUM+1][AXIS_NUM+1];
	float P[AXIS_NUM+1][AXIS_NUM+1];
	float K[AXIS_NUM+1][AXIS_NUM];
	float Q[AXIS_NUM+1][AXIS_NUM+1];
	float R1[AXIS_NUM][AXIS_NUM];
	float R2[AXIS_NUM][AXIS_NUM];
	float Offset[AXIS_NUM];	 
	float Range[AXIS_NUM];
	float CalibratedValue[AXIS_NUM+1][AXIS_NUM];	
  Bool GetMaxMin;
} SensorStructEKF;

typedef struct
{
	unsigned char Buff[AXIS_NUM * 2];
	float Raw[AXIS_NUM];
	float	Kalman[AXIS_NUM];
	float Max[AXIS_NUM];
	float Min[AXIS_NUM];
	float P_Kalman[AXIS_NUM];
	float K_Kalman[AXIS_NUM];
	float Q_Kalman[AXIS_NUM];
	float R_Kalman[AXIS_NUM];
	float Offset[AXIS_NUM];	 
	float Range[AXIS_NUM];
	float CalibratedValue[AXIS_NUM];	
  Bool GetMaxMin;
} SensorStruct;

void IMU_Init(void);					
void IMU_RawData(void);
void IMU_Kalman(void);
void IMU_Run(void);					 
void IMU_SendAll(USART_TypeDef* USARTx);
void IMU_Send(USART_TypeDef* USARTx, unsigned char id, float send_value);
void I2C_Configuration(void);
static void I2C_AcknowledgePolling(I2C_TypeDef *I2Cx,uint8_t I2C_Addr);
uint8_t I2C_Read(I2C_TypeDef *I2Cx, uint8_t I2C_Addr, uint8_t addr, uint8_t *buf, uint16_t num);
uint8_t I2C_WriteOneByte(I2C_TypeDef *I2Cx,uint8_t I2C_Addr,uint8_t addr,uint8_t value);	

void Meg_Init(void);														
void Meg_Fixed_MaxMin(void);
void Meg_Kalman(void);		
void CalibrateMeg(void);
void RM146(void);			 
double get_Pitch(double *gval);			 
double get_Roll(double *gval);
double GetAzimuth(double *mval, double pitch, double roll);
double GetCalibratedAzimuth(double *mval, double pitch, double roll);

//Matrix operate
void Transpose(int RowNum, int ColNum, mtype* A, mtype* B); // B = Transpose(A)
void Plus(int RowNum, int ColNum, mtype* A, mtype* B, mtype* C); // C = A + B
void Minus(int RowNum, int ColNum, mtype* A, mtype* B, mtype* C); // C = A - B
void Identity(int RowNum, mtype* A); // A = I;
Bool Inverse2(mtype* A, mtype* B); // B = inv(A)
Bool Inverse3(mtype* A, mtype* B); // B = inv(A)
Bool Multiply(int RowA, int ColA, int RowB, int ColB, mtype* A, mtype* B, mtype* C); // C = A * B
void ScalarMultiply(mtype scalar, int RowNum, int ColNum, mtype* A, mtype* B); // B = a * A

#endif
